#include "app_cfg.h"
#include "app_type.h"

#include "interfaces.h"
#include "tool/buffer/buffer.h"
#include "framework/vsfsm/vsfsm.h"
#include "framework/vsftimer/vsftimer.h"
#include "dal/usart_stream/usart_stream.h"

#include "comm.h"
#include "sensor.h"

struct sensor_uploader_t
{
	uint16_t dummy;
	uint16_t hmc5883l_x;
	uint16_t hmc5883l_y;
	uint16_t hmc5883l_z;
	
	int32_t mpu6050_accel_x;
	int32_t mpu6050_accel_y;
	int32_t mpu6050_accel_z;
	int32_t mpu6050_gyro_x;
	int32_t mpu6050_gyro_y;
	int32_t mpu6050_gyro_z;
	int32_t mpu6050_temp;
			
	int32_t bmp280_temp;
	int32_t bmp280_press;
	
	uint32_t checksum;
};

void sensor_uploader_init(void)
{	
	usart_stream_init(&uart1_info);
	usart_stream_config(&uart1_info);
}

void sensor_uploader_int_callback(struct sensor_t *sensor)
{
	uint32_t i;
	uint32_t *p;
	struct sensor_uploader_t up;
		
	if (sensor->poll && !sensor->error)
	{
		up.dummy = 0xabcd;
		up.hmc5883l_x = sensor->hmc5883l.x_lsb + ((uint16_t)sensor->hmc5883l.x_msb << 8);
		up.hmc5883l_y = sensor->hmc5883l.y_lsb + ((uint16_t)sensor->hmc5883l.y_msb << 8);
		up.hmc5883l_z = sensor->hmc5883l.z_lsb + ((uint16_t)sensor->hmc5883l.z_msb << 8);
		
		up.bmp280_temp = sensor->bmp280.temp;
		up.bmp280_press = sensor->bmp280.press;
		
		memcpy(&up.mpu6050_accel_x, &sensor->mpu6050.accel_x, 28);
		
		up.checksum = 0;
		p = (uint32_t *)&up;
		for (i = 0; i <= sizeof(struct sensor_uploader_t) / 4 - 1; i++)
			up.checksum += p[i];
		
		vsf_fifo_push(&uart1_info.stream_tx.fifo, sizeof(struct sensor_uploader_t), (uint8_t *)&up);
		usart_stream_send_bytes(&uart1_info);
	}
}

